#include "output.h"

void OutputModule::SendCommands(int Y, int X){

    cout << "Sending move command" << endl;

    if(!FirstTime){
      CurrentPositionX = X;
      FirstTime = true;
    }
    else{
      if(X < CurrentPositionX){
         cout << "Moving left" << endl;
         motorA->on(-10);
         Sleep(150);
         motorA->off();
      }
      else if(X > CurrentPositionX){
         cout << "Moving right" << endl;
          motorA->on(10);
         Sleep(150);
         motorA->off();
      }
      else if(X == CurrentPositionX){
         cout << "Move nothing."  << endl;
      };
    };
    CurrentPositionX = X;

};

void OutputModule::Tracking(int Y, int X){
      if(X < (CamCentreX - 15)){
         //cout << "Moving left" << endl;
         input ="1";
         nxt->write_msg(input, 0, false);
      }
      else if(X > (CamCentreX + 15)){
         //cout << "Moving right" << endl;
         input ="2";
         nxt->write_msg(input, 0, false);

      };

      while(!CommandBack){

        try{
          //Sleep(1);
          output = nxt->read_msg(1,false);

        if(output[0] == '1'){
            CommandBack = true;
          };

      }catch(Nxt_exception& e){
          CommandBack = true;
    };
      };
      output[0] = '0';
      input ="0";
      CommandBack = false;
};

void OutputModule::Tracking2(int Y, int X){
      if(X < (CamCentreX - 15)){
         cout << "Moving left" << endl;
         input ="1";
         nxt->write_msg(input, 0, false);
      }
      else if(X > (CamCentreX + 15)){
         cout << "Moving right" << endl;
         input ="2";
         nxt->write_msg(input, 0, false);

      }else{
        input ="1";
        nxt->write_msg(input2, 3, true);
        cout << "==================================" << endl;
        Sleep(1000);
      };

      while(!CommandBack){

        try{
          output = nxt->read_msg(1,false);

        if(output[0] == '1'){
            CommandBack = true;
          };

      }catch(Nxt_exception& e){
          CommandBack = true;
    };
      };
      output[0] = '0';
      input ="0";
      CommandBack = false;
};

void OutputModule::SetupCom(int Width, int Heigth){

    cout << "Width: " << Width << endl;
    if((Width != 0) || (Heigth != 0)){
      PixelAngleRatio = 180.00 / (double) Width;
      CamCentreX = Width / 2;
      CamCentreY = Heigth / 2;
      cout << "Centre X is: " << CamCentreX << endl;
      cout << "Centre Y is: " << CamCentreY << endl;
    cout << "PixelAngleRatio: " << PixelAngleRatio << endl;
    }
    connection = new Bluetooth();
    nxt = new Brick(connection);
    motorA = new Motor(OUT_A, connection);
    connection->connect(3);
};
